#include "camera.h"
#include <GL/glu.h>
#include <iostream>
#include <math.h>
using namespace std;
using namespace d_Math;

Camera::Camera()
{
    mStartRot.make_diag();
    mCurrentRot.make_diag();
	mMoving=false;
}

void Camera::SetDimensions(int w, int h)
{
    mDimensions[0] = w;
    mDimensions[1] = h;
}

void Camera::SetPerspective(double fovy, double near, double far)
{
    mPerspective[0] = fovy;
	mPerspective[2] = near;
	mPerspective[3] = far;
}

void Camera::SetViewport(int x, int y, int w, int h)
{
    mViewport[0] = x;
    mViewport[1] = y;
    mViewport[2] = w;
    mViewport[3] = h;
    mPerspective[1] = double(w)/h;
}

void Camera::SetCenter(const d_Vec3f &center)
{
    mStartCenter = mCurrentCenter = center;
}

void Camera::SetRotation(const d_Mat4f &rotation)
{
    mStartRot = mCurrentRot = rotation;
}

void Camera::SetDistance(const double distance)
{
    mStartDistance = mCurrentDistance = distance;
}

void Camera::MouseClick(Button button, int x, int y)
{
    mStartClick[0] = x;
    mStartClick[1] = y;

    mButtonState = button;
    switch (button)
    {
    case LEFT:
        mCurrentRot = mStartRot;
        break;
    case MIDDLE:
        mCurrentCenter = mStartCenter;
        break;
    case RIGHT:
        mCurrentDistance = mStartDistance;
        break;        
    default:
        break;
    }
	mMoving = true;
}

void Camera::MouseDrag(int x, int y)
{
    switch (mButtonState)
    {
    case LEFT:
        ArcBallRotation(x,y);
        break;
    case MIDDLE:
        PlaneTranslation(x,y);
        break;
	case RIGHT:
        DistanceZoom(x,y);
        break;
    default:
        break;
    }
}


void Camera::MouseRelease(int x, int y)
{
    mStartRot = mCurrentRot;
    mStartCenter = mCurrentCenter;
    mStartDistance = mCurrentDistance;
    mMoving = false;
    mButtonState = NONE;
}


void Camera::ArcBallRotation(int x, int y)
{
    double sx, sy, sz, ex, ey, ez;
    double scale;
    double sl, el;
    double dotprod;
    
    // find vectors from center of window
    sx = mStartClick[0] - (mDimensions[0]  / 2);
    sy = mStartClick[1] - (mDimensions[1] / 2);
    ex = x -   (mDimensions[0] / 2);
    ey = y -   (mDimensions[1] / 2);
    
    // invert y coordinates (raster versus device coordinates)
    sy = -sy;
    ey = -ey;
    
    // scale by inverse of size of window and magical sqrt2 factor
    if (mDimensions[0] > mDimensions[1]) {
        scale = (double) mDimensions[1];
    } else {
        scale = (double) mDimensions[0];
    }

    scale = 1.0/scale;
    
    sx *= scale;
    sy *= scale;
    ex *= scale;
    ey *= scale;

    // project points to unit circle
    sl = hypot(sx, sy);
    el = hypot(ex, ey);
    
    if (sl > 1.0) {
        sx /= sl;
        sy /= sl;
        sl = 1.0;
    }
    if (el > 1.0) {
        ex /= el;
        ey /= el;
        el = 1.0;
    }
    
    // project up to unit sphere - find Z coordinate
    sz = sqrt(1.0 - sl * sl);
    ez = sqrt(1.0 - el * el);
    
    // rotate (sx,sy,sz) into (ex,ey,ez)
    
    // compute angle from dot-product of unit vectors (and double
    // it).  compute axis from cross product.
    dotprod = sx * ex + sy * ey + sz * ez;

    if (dotprod != 1)
    {
        mCurrentRot.make_hrot( norm(d_Vec3f(sy * ez - ey * sz,
                                         sz * ex - ez * sx,
                                         sx * ey - ex * sy)),
                              2.0 * acos(dotprod) );
        mCurrentRot = mCurrentRot*mStartRot;
    }
    else
    {
        mCurrentRot = mStartRot;
    }


}

void Camera::PlaneTranslation(int x, int y)
{
    // map window x,y into viewport x,y
    // start
    int sx = mStartClick[0] - mViewport[0];
    int sy = mStartClick[1] - mViewport[1];

    // current
    int cx = x - mViewport[0];
    int cy = y - mViewport[1];


    // compute "distance" of image plane (wrt projection matrix)
    double d = double(mViewport[3])/2.0 / tan(mPerspective[0]*3.141592 / 180.0 / 2.0);

    // compute up plane intersect of clickpoint (wrt fovy)
    double su = -sy + mViewport[3]/2.0;
    double cu = -cy + mViewport[3]/2.0;

    // compute right plane intersect of clickpoint (ASSUMED FOVY is 1)
    double sr = (sx - mViewport[2]/2.0);
    double cr = (cx - mViewport[2]/2.0);

    d_Vec3f move(cr-sr, cu-su, 0);

    // this maps move
    move = move*(-mCurrentDistance/d);

    mCurrentCenter = mStartCenter +
         d_Vec3f(mCurrentRot(0,0),mCurrentRot(0,1),mCurrentRot(0,2))*move[0]
        + d_Vec3f(mCurrentRot(1,0),mCurrentRot(1,1),mCurrentRot(1,2))*move[1];
}

void Camera::ApplyViewport() const
{
    glViewport(mViewport[0],mViewport[1],mViewport[2],mViewport[3]);
}

void Camera::ApplyPerspective() const
{
	glMatrixMode(GL_PROJECTION);
    gluPerspective(mPerspective[0], mPerspective[1], mPerspective[2], mPerspective[3]);   
}


void Camera::ApplyModelview()
{
    // back up distance
    gluLookAt(0,0,mCurrentDistance,
              0,0,0,
              0.0, 1.0, 0.0);

    // rotate object
	float *tmp;
	mCurrentRot.ref(tmp);
    glMultMatrixf(tmp);

    //translate object to center
   glTranslatef(-mCurrentCenter[0],-mCurrentCenter[1],-mCurrentCenter[2]);    
}

d_Vec3f Camera::GetUp()
{
	d_Vec4f tmp =  trans(mCurrentRot)*d_Vec4f(0,1,0,0);
	return d_Vec3f(tmp[0], tmp[1], tmp[2]);
}

d_Vec3f Camera::GetPosition()
{
	d_Vec4f tmp = trans(mCurrentRot)*d_Vec4f(0,0,mCurrentDistance,0);
	return GetCenter()+d_Vec3f(tmp[0], tmp[1], tmp[2]);
}

void Camera::DistanceZoom(int x, int y)
{
    int sy = mStartClick[1] - mViewport[1];
    int cy = y - mViewport[1];

    double delta = double(cy-sy)/mViewport[3];
    // exponential zoom factor
    mCurrentDistance = mStartDistance * exp(delta);  
}

